#if 1

/**
 * @file imulib.c
 * @author Yifan Xiao (1993996310@qq.com)
 * @brief IMU 姿态解算库, 算法参考: https://blog.csdn.net/u010097644/article/details/70881395
 * @version 0.1
 * @date 2023-03-09
 *
 * @copyright Copyright (c) 2023
 *
 */
#include "imulib.h"
// #include "headfile.h"


#define PI 3.1415926f

IMUContext imu_ctx;
IMUData imu_input;
EulerAngle imu_euler;


static void IMU_Get_Value(IMUData *Origin_Value)
{   	
    ask_accdata();        //获取加速度数据
    ask_gyro();            //获取陀螺仪数据
	 // 加速度计数据赋值
    Origin_Value->acc[0]=(float)mpu_acc_x / 16384;
    Origin_Value->acc[1]=(float)mpu_acc_y / 16384;
    Origin_Value->acc[2]=(float)mpu_acc_z / 16384;
	
	 // 陀螺仪数据赋值
    Origin_Value->gyr[0]=(float)mpu_gyro_x / 16.4f; 
    Origin_Value->gyr[1]=(float)mpu_gyro_x / 16.4f; 
    Origin_Value->gyr[2]=(float)mpu_gyro_x / 16.4f; 
}


static void IMU_Solve(IMUContext *imu_context, IMUData *imu_data, EulerAngle *euler_angle, float dt)
{
    float q0 = imu_context->q0;
    float q1 = imu_context->q1;
    float q2 = imu_context->q2;
    float q3 = imu_context->q3;

    float exInt = imu_context->exInt;
    float eyInt = imu_context->eyInt;
    float ezInt = imu_context->ezInt;

    float Kp = imu_context->Kp;
    float Ki = imu_context->Ki;

    float ax = imu_data->acc[0];
    float ay = imu_data->acc[1];
    float az = imu_data->acc[2];

    float gx = imu_data->gyr[0];
    float gy = imu_data->gyr[1];
    float gz = imu_data->gyr[2];

    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    if (ax * ay * az == 0)
    {
        return;
    }

    // 对原始数据进行单位转换
    gx = gx * PI / 180.0f;
    gy = gy * PI / 180.0f;
    gz = gz * PI / 180.0f;

    // 用加速度计测量的重力向量和地球重力加速度的叉积来计算误差
    norm = sqrt(ax * ax + ay * ay + az * az);
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;

    vx = 2 * (q1 * q3 - q0 * q2);
    vy = 2 * (q0 * q1 + q2 * q3);
    vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // 积分误差
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    // 调整陀螺仪测量的角速度
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

    // 四元数微分方程
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * (0.5f * dt);
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * (0.5f * dt);
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * (0.5f * dt);
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * (0.5f * dt);

    // 归一化四元数
    norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    // 更新四元数
    imu_context->q0 = q0;
    imu_context->q1 = q1;
    imu_context->q2 = q2;
    imu_context->q3 = q3;

    // 更新欧拉角
    euler_angle->roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / PI;
    euler_angle->pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / PI;
    euler_angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / PI;

    // 更新误差积分
    imu_context->exInt = exInt;
    imu_context->eyInt = eyInt;
    imu_context->ezInt = ezInt;
}


void IMU_Init(IMUContext *imu_context, IMUData *imu_data, EulerAngle *euler_angle)
{
    simiic_init();
    mpu6050_init();


    // 初始化四元数
    imu_context->q0 = 1.0f;
    imu_context->q1 = 0.0f;
    imu_context->q2 = 0.0f;
    imu_context->q3 = 0.0f;

    // 初始化误差积分
    imu_context->exInt = 0.0f;
    imu_context->eyInt = 0.0f;
    imu_context->ezInt = 0.0f;

    // 初始化增益
    imu_context->Kp = 2.0f;
    imu_context->Ki = 0.05f;

    // 初始化欧拉角
    euler_angle->roll = 0.0f;
    euler_angle->pitch = 0.0f;
    euler_angle->yaw = 0.0f;

    // 初始化加速度计数据
    imu_data->acc[0] = 0.0f;
    imu_data->acc[1] = 0.0f;
    imu_data->acc[2] = 0.0f;

    // 初始化陀螺仪数据
    imu_data->gyr[0] = 0.0f;
    imu_data->gyr[1] = 0.0f;
    imu_data->gyr[2] = 0.0f;

}







/**
 * @brief IMU 主要处理函数
 * @param 
 * @param 
 */
void IMU_Handler(uint16_t cycle_tick)
{
    static uint32_t TimeTick;
    uint32_t TimePass = (GetTick()-TimeTick);
    if(TimePass > cycle_tick);
    {
        TimeTick = GetTick();

        IMU_Get_Value(&imu_input);	
        IMU_Solve(&imu_ctx, &imu_input, &imu_euler, TimePass/1000.0f);
    }
}


#endif


